#include "mazesolver.h"

using std::vector;

/**
 * Methods for the Coordinates class
 */

Coordinates::Coordinates(int x, int y, int z):
	x(x), y(y), z(z)
{
}

bool Coordinates::operator==(const Coordinates& otherCoordinates) const {
	return (x == otherCoordinates.x) && (y == otherCoordinates.y) && (z == otherCoordinates.z);
}

bool Coordinates::operator!=(const Coordinates& otherCoordinates) const {
	return (x != otherCoordinates.x) || (y != otherCoordinates.y) || (z != otherCoordinates.z);
}

int Coordinates::operator[](const int index) const {
	switch(index)
	{
	case 0:
		return x;
		break;
	case 1:
		return y;
		break;
	case 2:
		return z;
		break;
	default:
		exit(-1);
	}
}

/**
 * Methods for the MazeCell class
 */

MazeCell::MazeCell(unsigned char pixelValue) :
	pixelValue(pixelValue)
{
	this->value = (pixelValue == 0) ? VALUE_FILLED : VALUE_EMPTY;
}


void MazeCell::setValue(int value) {
	this->value = value;
}

int MazeCell::getValue() {
	return value;
}

unsigned char MazeCell::getPixelValue() {
	return pixelValue;
}

/**
 * Methods for the Maze class
 */

Maze::Maze(const int xSize, const int ySize, const int zSize, unsigned char ***mazeGrid):
	xSize(xSize),
	ySize(ySize),
	zSize(zSize)
{
	dataGrid = new MazeCell**[xSize];

	for(int i=0; i < xSize; i++)
	{
		dataGrid[i] = new MazeCell*[ySize];

		for(int j=0; j < ySize; j++)
		{
			dataGrid[i][j] = new MazeCell[zSize];

			for(int k=0; k < zSize; k++)
			{
				dataGrid[i][j][k] = MazeCell(mazeGrid[i][j][k]);
			}
		}
	}
}

Maze::~Maze()
{
	for(int i=0; i < xSize; i++)
	{
		for(int j=0; j < ySize; j++)
		{
			delete[] dataGrid[i][j];
		}

		delete[] dataGrid[i];
	}

	delete[] dataGrid;
};

/**
 * This methods retrieves a cell from the maze by its coordinate.
 */
MazeCell* Maze::getCell(Coordinates c)
{
	if(0<=c.x && c.x<xSize && 0<= c.y && c.y<ySize && 0<=c.z && c.z<zSize)
		return &dataGrid[c.x][c.y][c.z];
	return 0;
}

/*
 * This methods returns all accessible neighbors of a cell in the maze.
 */
std::vector<MazeCellCoordinated> Maze::getNeighbors(MazeCellCoordinated &cc)
{

	/*
		Possible path neighboring pixels for pixel (i,j,k) are
		(i,j-1,k-1), (i,j-1,k+1), (i, j+1,k-1), (i,j+1,k+1), (i-1,j,k), (i+1,j,k)
	*/

	const int dirs[6][3] = 
	{
		{-1, 0, 0},
		{1, 0, 0},
		{0, -1, -1},
		{0, -1, 1},
		{0, 1, -1},
		{0, 1, 1},
	};

	std::vector<MazeCellCoordinated> neighbors;
    int Coordinates::*u[3] = {&Coordinates::x, &Coordinates::y, &Coordinates::z};

	for(int i=0; i<6; i++) {
		Coordinates c = cc.coords;

		for(int j=0; j<3; j++) {
			c.*u[j] += dirs[i][j];
		}

		MazeCell *neighbor = getCell(c);
		int value;
		if(neighbor && (value = neighbor->getValue()) != VALUE_FILLED) { // only the accessible neighbors!
			neighbors.push_back(MazeCellCoordinated(neighbor, c));
		}
	}

    return neighbors;
}

/**
* The solve method solves the maze and returns all valid paths
*/
Paths Maze::solve(Coordinates &entranceCoordinates, Coordinates &exitCoordinates)
{
	// initialize queue
	std::queue<MazeCellCoordinated> nextCells;

	// mark starting cell with its pixel value and add it to the queue
	MazeCell *end = getCell(exitCoordinates);
	MazeCell *start = getCell(entranceCoordinates);

	nextCells.push(MazeCellCoordinated(start, entranceCoordinates));
		 
	// move to next cells until there are none left
	while(!nextCells.empty()) {
		// get the next cell (= front of queue)
		MazeCellCoordinated currentCC = nextCells.front();//.top();
		nextCells.pop();
		
		if (currentCC.cell == end) {
			// do nothing, since we still need the other possible paths
		} else {
			// getting the neighbors
			std::vector<MazeCellCoordinated> neighbors = getNeighbors(currentCC);
			
			// add all untouched neighbors to the queue, or resolve collisions
			for each (MazeCellCoordinated neighbor in neighbors) {
				if(neighbor.cell->getValue() == VALUE_EMPTY) { // untouched
					int nextValue = currentCC.cell->getValue() + 1;
					neighbor.cell->setValue(nextValue);

					nextCells.push(neighbor); // add untouched neighbors to queue
				} else if (!isJunction(neighbor)) { // check if neighbor is no junction
					resolveCollision(currentCC, neighbor);
				}
			}
		}
	}

	Paths results; // collection to keep all results

	std::queue<PartialSolution> startpointQueue; // this queue is used to keep all partial solutions (with their starting point)
	startpointQueue.push(PartialSolution(MazeCellCoordinated(end, exitCoordinates))); // the exit of the maze is the first entry in this queue
	
	while(!startpointQueue.empty()) { // repeat until there are no unfinished partial solutions
		PartialSolution partial = startpointQueue.front(); // get the head of the queue
		startpointQueue.pop();

		MazeCellCoordinated cc = partial.cc; // fetch all data of the partial solution
		Path res = partial.res;
		bool shortestPathCoordinateFound = true; // helper boolean to check if the shortest path was already found
		while((cc.cell != start) && (cc.cell->getValue() != VALUE_FILLED_JUNCTION) && shortestPathCoordinateFound) { // repeat until ending point (= start) is reached
			// creates a new segment on the current path (is used for the visualization of the path)
			Segment s;
			s.point[0] = (float) cc.coords.x;
			s.point[1] = (float) cc.coords.y;
			s.point[2] = (float) cc.coords.z;
			s.scalar = cc.cell->getPixelValue();
			res.push_back(s);

			// calculate the value of the next cell on the current path
			int value = cc.cell->getValue() - 1;

			// fetch the neighbors
			vector<MazeCellCoordinated> neighbors = getNeighbors(cc);

			shortestPathCoordinateFound = false; // this is set to false each iteration
			
			for each (MazeCellCoordinated n in neighbors) {
				if ((n.cell->getValue() == value) && !shortestPathCoordinateFound) { // a cell was found on the current path
					// fill the visited cells
					if(isJunction(cc)) {
						cc.cell->setValue(VALUE_FILLED_JUNCTION); // junctions get a special value so they can be reached by alternative paths
					} else {
						cc.cell->setValue(VALUE_FILLED);
					}
					cc = n;
					shortestPathCoordinateFound = true;
				} else if ((n.cell->getValue() == value) || hasValidPath(cc.coords, n)) { // check if neighbor has a valid alternative path
					// these either alternative shortest or longer valid paths, add the partial solutions of them on the queue
					Path newResult;
					newResult.push_back(res.back());
					startpointQueue.push(PartialSolution(n, newResult));
				}
			}
		}

		if (cc.cell == start) {
			// creates a final segment on the current path (is used for the visualization of the path)
			Segment startSegment;
			startSegment.point[0] = (float) entranceCoordinates.x;
			startSegment.point[1] = (float) entranceCoordinates.y;
			startSegment.point[2] = (float) entranceCoordinates.z;
			startSegment.scalar = start->getPixelValue();
			res.push_back(startSegment);
		}
		if ((res.size() > 2) || (cc.cell == start)) { // last check is for the special case when entrance and exit are neighbors

			std::reverse(res.begin(), res.end());

			results.push_back(res);
		}
	}
	return results; 
}

/**
 * The following methods are used to check or retrieve local information near a cell or in a corridor.
 * They are used during the solve operation.
 */
MazeCellCoordinated& Maze::getLastJunction(MazeCellCoordinated &cc) {
	while (true) {
		vector<MazeCellCoordinated> neighbors = getNeighbors(cc);
		if (neighbors.size() > 1) { // is junction
			break;
		} else if(neighbors.size() == 1) {
			cc = neighbors[0];
		} else {
			exit(-1);
		}
	}
	return cc;
}

/**
 * Check if a cell is a junction
 */
bool Maze::isJunction(MazeCellCoordinated &cc) {
	vector<MazeCellCoordinated> neighbors = getNeighbors(cc);
	return neighbors.size() > 2;
}

/**
 * Helper function to resolve collisions
 */
void Maze::resolveCollision(MazeCellCoordinated &ccA, MazeCellCoordinated &ccB) {
	MazeCellCoordinated& lastJunctionA = getLastJunction(ccA);
	MazeCellCoordinated& lastJunctionB = getLastJunction(ccB);
	if (lastJunctionA.cell->getValue() < lastJunctionB.cell->getValue()) {
		overwriteCorridor(ccA, ccB, lastJunctionB.coords);
	} else {
		overwriteCorridor(ccB, ccA, lastJunctionA.coords);
	}
}

/**
 * Helper function to overwrite corridors
 */
void Maze::overwriteCorridor(MazeCellCoordinated &previousCell, MazeCellCoordinated &currentCell, Coordinates &endCoordinates) {
	int value = previousCell.cell->getValue();
	while(currentCell.coords != endCoordinates) {
		value++;
		currentCell.cell->setValue(value);
		vector<MazeCellCoordinated> neighbors = getNeighbors(currentCell);
		for each (MazeCellCoordinated neighbor in neighbors) {
			if(neighbor.coords != previousCell.coords) {
				previousCell = currentCell;
				currentCell = neighbor;
				break;
			}
		}
	}
}

/**
 * Check if alternative path has a valid path to the entrance
 */
bool Maze::hasValidPath(Coordinates &previousCoordinates, MazeCellCoordinated &cc) {
	bool result = false;
	vector<MazeCellCoordinated> neighbors = getNeighbors(cc);
	for each (MazeCellCoordinated neighbor in neighbors) {
		int value = cc.cell->getValue() - 1;
		if((neighbor.coords != previousCoordinates) && (neighbor.cell->getValue() == value)) {
			result = true;
			break;
		}
	}
	return result;
}